import sys
import threading
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
from PyQt5 import QtCore, QtGui, QtWidgets
from PyQt5.QtWidgets import QFileDialog,QWidget
from PyQt5.QtCore import Qt , QPoint
import numpy as np
class clickableQlabel(QtWidgets.QLabel):
    def __init__(self, parent=None):
        super(clickableQlabel, self).__init__(parent)
        self.point_list=[]
    def mousePressEvent(self, event) :   
    # 鼠标左键按下
        if event.button() == Qt.LeftButton :
            self.lastPoint = event.pos()
            self.endPoint = self.lastPoint
            #print(event.pos())
            self.point_list.append((event.pos().x(), event.pos().y()))
    
class Ui_Detect(QWidget):
    def __init__(self):
        super().__init__()
        self.isCapture = False
        self.open_camera = QtWidgets.QPushButton(self)
        self.open_camera.clicked.connect(self.open)
        self.open_camera.setText("启动相机")
        self.open_camera.move(1920,0)
        self.open_camera.show()

        self.clear=QtWidgets.QPushButton(self)
        self.clear.clicked.connect(self.clear_point_list)
        self.clear.setText("清除")
        self.clear.move(1920,30)
        self.clear.show()

        self.calc=QtWidgets.QPushButton(self)
        self.calc.clicked.connect(self.output_pos)
        self.calc.setText("计算")
        self.calc.move(1920,60)
        self.calc.show()

        self.label = clickableQlabel(self)
        self.label.setText("")
        self.label.setObjectName("label")
        self.label.move(0,0)
        self.label.show()
    def draw_point_list(self,img,list):
        for (id,point) in enumerate(list):
            cv2.circle(img, (round(point[0]), round(point[1])), 2, (0, 255, 255), 2)
            #cv2.putText(img,str(id),(point[0], point[1]),cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        return img
    def aruco_search(self,image):
        arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
        arucoParams = cv2.aruco.DetectorParameters_create()
        (corners, ids, rejected) = cv2.aruco.detectMarkers(image, arucoDict,parameters=arucoParams)
        return corners

    def open(self):
        print("打开摄像头")
        cap = cv2.VideoCapture(0)
        cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
        cap.set(cv2.CAP_PROP_FRAME_WIDTH,1920)
        while cap.isOpened():
            ret, frame = cap.read()
            self.image=frame
            #print(self.aruco_search(frame))

            arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
            arucoParams = cv2.aruco.DetectorParameters_create()
            (corners, ids, rejected) = cv2.aruco.detectMarkers(self.image, arucoDict,parameters=arucoParams)

            frame=cv2.aruco.drawDetectedMarkers(frame,corners,ids)

            frame = self.draw_point_list(frame,self.label.point_list)

            img = QtGui.QImage(frame.data, frame.shape[1], frame.shape[0], QtGui.QImage.Format_BGR888)
            self.label.setPixmap(QtGui.QPixmap.fromImage(img))
            self.label.resize(1920,1080)
            cv2.waitKey(0)
    def clear_point_list(self):
        self.label.point_list.pop()
    
    def output_pos(self):
        #assert len(self.label.point_list)>=4
        #imagePoints=np.array([(1000, 802), (1435, 675), (896, 436), (1237, 222)]).astype('float32')
        imagePoints=np.array(self.label.point_list).astype('float32')
        objectPoints=np.array([[0,0,0],[0,800,0],[800,0,0],[800,800,0]]).astype('float32')
        cameraMatrix=np.array( [[894.348826   ,  0,         936.44500723],
        [  0,         896.42857334, 558.08348431],
        [  0,           0,           1,        ]])
        distCoeffs=np.array([0,0,0,0,0]).astype('float32')
        retval,rvet,tvet=cv2.solvePnP(objectPoints,imagePoints,cameraMatrix,distCoeffs)
        print('rvet',rvet)
        print('tvet',tvet)
        w=11
        h=11
        objp = np.zeros((w*h,3), np.float32)
        objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
        objp=objp*80
        re,jac=cv2.projectPoints(objp,rvet,tvet,cameraMatrix,distCoeffs)
        re=re.reshape(-1,2)
        print(re)
        img=self.draw_point_list(self.image,re)
        cv2.imshow('img',img)
        cv2.waitKey(0)



if __name__ == "__main__":
    app = QtWidgets.QApplication(sys.argv)
    window = QtWidgets.QWidget()
    ui = Ui_Detect()
    ui.show()
    sys.exit(app.exec_())
